﻿using System;
using System.Threading;
using System.Collections.Generic;
using Microsoft.Xna.Framework;
using Microsoft.Xna.Framework.Audio;
using Microsoft.Xna.Framework.Content;
using Microsoft.Xna.Framework.GamerServices;
using Microsoft.Xna.Framework.Graphics;
using Microsoft.Xna.Framework.Input;
using Microsoft.Xna.Framework.Net;
using Microsoft.Xna.Framework.Storage;
using WiimoteLib;
using InTheHand.Net;
using InTheHand.Net.Bluetooth;
using InTheHand.Net.Sockets;


namespace Advanced_Terrain
{
    public class WiiMoteConnector
    {
        public Boolean IsConnected;
        //public Boolean Calibrating;
        //private Boolean Calibrated;
        private Vector3 A = new Vector3(0, 0, 0);
        private Vector3 G = new Vector3(0, 0, 0);
        public Vector3 Angle = new Vector3(0, 0, 0);
        public Quaternion Rotation = Quaternion.Identity;
        //private Boolean CalibrationReset = false;
        private Vector3 AccelCalibration;
        private Vector3 GyroCalibration;
        //private float StartTimeMilli = 0;
        private Boolean FirstTime = true;
        public Boolean SuccessfulSwing = false;
        //private Boolean Bstate;
        public Wiimote wm = new Wiimote();
        //public float[,] MotionThrowArray;
        //public float[,] AccelThrowArray;
        private int ArrayNum = 0;
        private Vector3 SavedAngle = new Vector3(0,0,0);
       public void UpdateState(WiimoteChangedEventArgs args)
        {
            UpdateWiimoteChanged(args);
        }

        public void UpdateExtension(WiimoteExtensionChangedEventArgs args)
        {
            //UpdateExtensionChanged(args);
        }
        private void UpdateWiimoteChanged(WiimoteChangedEventArgs args)
        {

            WiimoteState ws = args.WiimoteState;


            /*
            clbButtons.SetItemChecked(0, ws.ButtonState.A);
            clbButtons.SetItemChecked(1, ws.ButtonState.B);
            clbButtons.SetItemChecked(2, ws.ButtonState.Minus);
            clbButtons.SetItemChecked(3, ws.ButtonState.Home);
            clbButtons.SetItemChecked(4, ws.ButtonState.Plus);
            clbButtons.SetItemChecked(5, ws.ButtonState.One);
            clbButtons.SetItemChecked(6, ws.ButtonState.Two);
            clbButtons.SetItemChecked(7, ws.ButtonState.Up);
            clbButtons.SetItemChecked(8, ws.ButtonState.Down);
            clbButtons.SetItemChecked(9, ws.ButtonState.Left);
            clbButtons.SetItemChecked(10, ws.ButtonState.Right);

            lblAccel.Text = ws.AccelState.Values.ToString();

            chkLED1.Checked = ws.LEDState.LED1;
            chkLED2.Checked = ws.LEDState.LED2;
            chkLED3.Checked = ws.LEDState.LED3;
            chkLED4.Checked = ws.LEDState.LED4;
            */
            switch (ws.ExtensionType)
            {

         
                case ExtensionType.MotionPlus:

                    if (ws.ButtonState.B)
                    {
                        if (FirstTime)
                        {
                            AccelCalibration = new Vector3(ws.AccelState.Values.X, ws.AccelState.Values.Y, ws.AccelState.Values.Z);
                            GyroCalibration = new Vector3(ws.MotionPlusState.RawValues.X, ws.MotionPlusState.RawValues.Y, ws.MotionPlusState.RawValues.Z);
                            Angle = new Vector3(0, 0, 0);
                            FirstTime = false;
                        }
                        ArrayNum++;
                        Rotation = CalculateRotation(ws);
                        
                    }
                    else
                    {
                        if (ArrayNum > 50)
                        {
                            SuccessfulSwing = true;
                            Game1.GameState = Game1.GameStateWiiMoteCalc;
                        }
                        else
                        {
                            Rotation = CalculateRotation(ws);
                            SuccessfulSwing = false;
                        }
                        FirstTime = true;
                        
                        ArrayNum = 0;
                        
            
                    }
                    
                    break;
            }

            //pbBattery.Value = (ws.Battery > 0xc8  0xc8 : (int)ws.Battery);
            //lblBattery.Text = ws.Battery.ToString();
            //lblDevicePath.Text = "Device Path: " + mWiimote.HIDDevicePath;
        }
        private void wm_WiimoteChanged(object sender, WiimoteChangedEventArgs args)
        {
            UpdateState(args);
        }

        private void wm_WiimoteExtensionChanged(object sender, WiimoteExtensionChangedEventArgs args)
        {
            UpdateExtension(args);

            if (args.Inserted)
                wm.SetReportType(InputReport.IRExtensionAccel, true);
            else
                wm.SetReportType(InputReport.IRAccel, true);
        }
        public void InitWiiMote()
        {
            Thread T = new Thread(SearchAndConnect);
            T.Start();
            //SearchAndConnect();
            
            
        }
        private Quaternion CalculateRotation(WiimoteState ws)
        {
            G.X = ws.MotionPlusState.RawValues.X - GyroCalibration.X;
            G.Y = ws.MotionPlusState.RawValues.Y - GyroCalibration.Y;
            G.Z = ws.MotionPlusState.RawValues.Z - GyroCalibration.Z;
            if (ws.MotionPlusState.RollFast)
            {
                G.X = ((G.X) / 4);
            }
            else
            {
                G.X = ((G.X) / 20);
            }
            if (ws.MotionPlusState.RollFast)
            {
                G.Y = ((G.Y) / 4);
            }
            else
            {
                G.Y = (G.Y / 20);
            }
            if (ws.MotionPlusState.RollFast)
            {
                G.Z = (G.Z / 4);
            }
            else
            {
                G.Z = (G.Z / 20);
            }
            Angle.Z = .98f * (Angle.Z + (G.Z * .015f)) + .02f * A.Z;
            Angle.X = .98f * (Angle.X + (G.X * .015f)) + .02f * A.X;
            Angle.Y = .98f * (Angle.Y + (G.Y * .015f)) + .02f * A.Y;
            return Quaternion.CreateFromYawPitchRoll(MathHelper.ToRadians(Angle.Z * -1), MathHelper.ToRadians(Angle.X), MathHelper.ToRadians(Angle.Y));

        }
        private void SearchAndConnect()
        {
            BluetoothClient client = null;
            //Game1.mainMenu.WiiMoteSearching.Add("Connecting");
            try
            {
                client = new BluetoothClient();

                BluetoothDeviceInfo[] devices = null;
                //UpdateStatus("Discovering Devices ...");
                devices = client.DiscoverDevices();
                //UpdateStatus("Discovery Complete");

                //ListDevicesFound(devices);
                //lstConnected.Items.Clear();
                //UpdateStatus("Connecting");

                for (int i = 0; i <= devices.Length - 1; i++)
                {

                    if ((devices[i].DeviceName.CompareTo("Nintendo RVL-CNT-01") == 0))
                    {
                        //If Found one
                        Game1.mainMenu.WiiMoteSearching.Add("Wiimote Found");
                        if ((devices[i].Connected))
                        {
                            //AddConnectedDevice(devices[i]);
                            Game1.mainMenu.WiiMoteSearching.Add("WiiMote installed Finalizing Connection");

                        }
                        else
                        {
                            //UpdateStatus("Connecting to Wii Remote:" + devices(i).DeviceAddress.ToString);
                            Game1.mainMenu.WiiMoteSearching.Add("Installing WiiMote as Windows Device");
                            Game1.mainMenu.WiiMoteSearching.Add("Release 1+2");


                            if ((devices[i].InstalledServices.Length == 0))
                            {
                                //If not installed its quick
                                devices[i].SetServiceState(BluetoothService.HumanInterfaceDevice, true);
                                //Should be connected now

                                //AddConnectedDevice(devices[i]);
                                System.Threading.Thread.Sleep(10000);
                            }
                            else
                            {
                                //If already installed, need to uninstall it then intall it again to get it to connect
                                //UpdateStatus("Uninstalling Wii Remote:" + devices(i).DeviceAddress.ToString);
                                BluetoothSecurity.RemoveDevice(devices[i].DeviceAddress);
                                //UpdateStatus("Connecting to Wii Remote:" + devices(i).DeviceAddress.ToString);
                                devices[i].SetServiceState(BluetoothService.HumanInterfaceDevice, true);
                                //Should Connect properly now

                                //AddConnectedDevice(devices[i]);
                                System.Threading.Thread.Sleep(15000);
                            }

                        }
                    }



                }
                // Non wii Devices here




                Game1.mainMenu.WiiMoteSearching.Add("Connecting to game (Windows Done)");

                wm.WiimoteChanged += wm_WiimoteChanged;
                wm.WiimoteExtensionChanged += wm_WiimoteExtensionChanged;
                Thread.Sleep(10000);
                try
                {
                    wm.Connect();
                    Game1.mainMenu.WiiMoteSearching.Add("Wiimote Connected");

                    Thread.Sleep(3000);
                    wm.SetReportType(InputReport.IRAccel, true);
                    wm.SetLEDs(true, true, true, true);
                }
                catch
                {
                    Game1.mainMenu.ErrorMessage = "Unable to find WiiMote";
                    Game1.mainMenu.SwitchMenu(0);
                    return;
                }
                try
                {

                    wm.InitializeMotionPlus();
                    Game1.mainMenu.WiiMoteSearching.Add("Motion++ Connected");

                    Game1.mainMenu.WiiMoteSearching.Add("Preparing to Calibrate");

                    wm.SetLEDs(false, true, true, false);
                    Thread.Sleep(3000);
                    this.IsConnected = true;
                    Game1.mainMenu.SwitchMenu(5);
                    if (Game1.MyKeyboard.WiiMoteConnected())
                    {

                        //Game1.MyWiimote.Calibrating = true;

                    }
                }
                catch
                {
                    //Unable to Find MotionPlus
                    Game1.mainMenu.ErrorMessage = "Unable to find MotionPlus";
                    Game1.mainMenu.SwitchMenu(0);
                    return;
                }
                

            }
        
        catch
    {
        if (Game1.mainMenu.ErrorMessage == "")
            Game1.mainMenu.ErrorMessage = "Unable to Initialize Bluetooth";
        Game1.mainMenu.SwitchMenu(0);
    }
        finally
    {
        
    }
            Game1.mainMenu.SwitchMenu(2);
   }
  }
}
